#include "mech_version_client.h"
#include "logger.h"
#include "utils.h"
#define CMD101_FORMAT "101,%d,0,1,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf"
#define CMD101_REPLY_FORMAT "%d,%d"
#define CMD102_FORMAT "102,%d"

mech_version_client::mech_version_client()
{

}

int mech_version_client::init()
{
    version_client.init(localPort, localIP);
    if(!version_client.connect())
    {
        LOG(ERROR) << "mech_version_client connect to server failed!!!\n";
        return false;
    }
    version_client.client_cb = std::bind(&mech_version_client::recv_data, this, std::placeholders::_1, std::placeholders::_2);
    return true;
}

/*
*@num(in): 运行工程的num
*@pose(in): 姿态
*@pos(in):轴角度
*@poses(out):返回的结果
*/
int mech_version_client::exe_project_get_pos(int num, MECH_VERSION_POSE pose, MECH_VERSION_POSE pos, std::vector<MECH_VERSION_POSE>& poses)
{
    //send 101 指令
    std::string cmd101 = get_cmd_101_str(num,pose,pos);
    evtVar.m_flag = false;
    version_client.sendMsg(cmd101);
    //3、阻塞接收
    if(false ==  evtVar.m_flag){
        int nRet = evtVar.wait(1000*30);
        if(nRet){
            LOG(ERROR)<<"evtVar wait out of time";
            return -1;
        }
    }
    int nRet = parse_cmd_101_reply_str();
    if(nRet != 1102){
        LOG(ERROR)<<"cmd 101 recv not 1102, nRet =" <<nRet<<std::endl;
        return -1;
    }

    //send 102 指令
    int do_cmd102 = 1;
    while(1 == do_cmd102){
        std::string cmd102 = get_cmd_101_str(num,pose,pos);
        evtVar.m_flag = false;
        version_client.sendMsg(cmd102);
        if(false ==  evtVar.m_flag){
            int nRet = evtVar.wait(1000*30);
            if(nRet){
                LOG(ERROR)<<"evtVar wait out of time";
                return -1;
            }
        }
        do_cmd102 = parse_cmd_102_reply_str(poses);
    }
    return 0;
}

int mech_version_client::recv_data(char* buf, size_t buf_size)
{
    memcpy(mData, buf, buf_size);
    mDataLen = buf_size;
    evtVar.notify();
    return 0;
}

std::string mech_version_client::get_cmd_101_str(int num, MECH_VERSION_POSE pose, MECH_VERSION_POSE pos)
{
    char buf[1024]={};
    //101, 1, 0, 1,                             //命令，工程号，结果点（0表示都要），位姿类型
    //5.18, 14.52, 4.03, 0.09, 72.44, 5.15,     //关节角
    //549.56, 50.0, 647.01, 180.0, -1.0, 180.0  //法兰位姿

    sprintf(buf,CMD101_FORMAT,num,\
            pos.p[0], pos.p[1], pos.p[2], pos.p[3], pos.p[4], pos.p[5],\
            pose.p[0], pose.p[1], pose.p[2], pose.p[3], pose.p[4], pose.p[5]);
    return std::string(buf);
}

int mech_version_client::parse_cmd_101_reply_str()
{
    int cmd,status;
    sscanf(mData,CMD101_REPLY_FORMAT, &cmd, &status);
    return status;
}

std::string mech_version_client::get_cmd_102_str(int num)
{
    //102, 1
    char buf[1024]={};
    sprintf(buf,CMD102_FORMAT,num);
    return std::string(buf);
}

int mech_version_client::parse_cmd_102_reply_str(std::vector<MECH_VERSION_POSE> &poses)
{
    std::string replyStr = std::string(mData,mDataLen);
    std::vector<std::string> replys = stringSplit(replyStr, ',');
    if(replys.size()<5){
        return -1;
    }
    if(replys[0] != "102"){
        return -1;
    }
    if(replys[1] != "1100"){
        return -1;
    }

    int nRet = stoi(replys[2]);
    int nSize= stoi(replys[3]);
    for(int i=0;i<nSize;i++){
        MECH_VERSION_POSE pose;
        pose.p[0] = stod(replys[i*8]);
        pose.p[1] = stod(replys[i*8+1]);
        pose.p[2] = stod(replys[i*8+2]);
        pose.p[3] = stod(replys[i*8+3]);
        pose.p[4] = stod(replys[i*8+4]);
        pose.p[5] = stod(replys[i*8+5]);
        poses.push_back(pose);
    }
    return nRet;
}

